import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    # 自动检测第一个 /dev/input/js* 设备
    js_devices = [f"/dev/input/{f}" for f in os.listdir("/dev/input") if f.startswith("js")]
    joy_dev = js_devices[0] if js_devices else "/dev/input/js0"

    # 加载参数文件路径
    config = os.path.join(
        get_package_share_directory('joystick_reader_py'),
        'config',
        'bingyuanlang.yaml'
    )

    # 启动 joy_node（读取手柄）
    joy_node = Node(
        package='joy',
        executable='joy_node',
        name='joy_node',
        parameters=[
            {'dev': joy_dev},
            {'deadzone': 0.2},
            {'coalesce_interval':0.02}
        ]
    )

    # 启动你自己的 joystick_reader_node
    reader_node = Node(
        package='joystick_reader_py',
        executable='joystick_motor',
        name='joystick_reader_py',
        parameters=[config]
    )

    motor_driver_node=Node(
        package='can_motor_driver',
        executable='winch_can_motor_driver_node',
        name='winch_can_motor_driver_node',
        output='screen',
        parameters=[
            {'serial_port_name': '/dev/ttyMotor'},
            {'control_frequency': 50.0},
            {'torque_filter_time': 1.0}
        ]
    )

    return LaunchDescription([
        joy_node,
        reader_node,
        motor_driver_node
    ])
